 ///////////////////////////////////////////////////////////////////////////////
//! @file	: face_detect.cl
//! @date   : 2016/03/16
//! @author: guyadong
//! @brief  : detect face in integral matrix
////////////////////////////////////////////////////////////////////////////////
#include "adaboost_face.h"
#include "rect_utilits.h"
#ifndef CL_DEVICE_LOCAL_MEM_SIZE
#error not defined CL_DEVICE_LOCAL_MEM_SIZE by complier with options -D
#endif

#ifndef V_TYPE
#error not defined V_TYPE by complier with options -D must be 2,4,8,16
#elif (V_TYPE!=16)&&(V_TYPE!=8)&&(V_TYPE!=4)&&(V_TYPE!=2)
#error error defined V_TYPE ,must be 2,4,8,16
#endif

#define VECTOR_FLOAT VECTOR(float,V_TYPE)
#define VECTOR_DOUBLE VECTOR(double,V_TYPE)
#define VECTOR_UCHAR VECTOR(uchar,V_TYPE)
#define VECTOR_CHAR VECTOR(char,V_TYPE)
#define VECTOR_USHORT VECTOR(ushort,V_TYPE)
#define VECTOR_LONG VECTOR(long,V_TYPE)
#define VECTOR_INT VECTOR(int,V_TYPE)

#define VECTOR_WEIGHT_TYPE VECTOR_FLOAT
#define SELECT_WEIGHT_TYPE VECTOR_INT

#define VLOAD FUN_NAME(vload,V_TYPE)
#define VSTORE FUN_NAME(vstore,V_TYPE)

/////////////////////////////////////////////////////////////////////////
inline float4 obj_normalization(
	 const int4 obj // xy: obj position,z: detected object sum of obj,w win_size
	,const float2 normalization_factor
	){
	float4 norm_obj=convert_float4(obj);
	norm_obj.xy*=normalization_factor;
	norm_obj.w=(float)(obj.w)*max(normalization_factor.x,normalization_factor.y);
	return norm_obj;
}
inline int4 obj_unnormalization(
	 const float4 norm_obj // xy: obj position,z: detected object sum of obj,w win_size
	,const int2 img_size
	){
	int4 obj=convert_int4(norm_obj);
	obj.xy=convert_int2(norm_obj.xy*convert_float2(img_size));
	obj.w=(int)(norm_obj.w*min(img_size.x,img_size.y));
	return obj;
}

///////////////////////////////////////////////////////////////////////////////
//! @brief  : return true if find intersectant rectangle with norm_obj in detected_objects_buffer_norm 
////////////////////////////////////////////////////////////////////////////////
inline bool has_intersect_norm(
	 const float4 norm_obj // xy: obj position,z: object sum of probe,w win_size
	,__global detected_objects_buffer_norm* detected_obj
	,float4 *intersect_obj
		){
	float4 rect1,rect2;
	for(int i=0;i<detected_obj->detected_num;++i){
		rect1=(float4)(norm_obj.xy,norm_obj.xy+norm_obj.w);		
		float4 detected_obj_i=vload4(i,(__global float*)detected_obj->storage);
		rect2=(float4)(detected_obj_i.xy,detected_obj_i.xy+detected_obj_i.w);
		if (is_intersectant_rectangle_norm(rect1,rect2)){			
			*intersect_obj=detected_obj_i;
			return true;
		}		
	}
	return false;
}

///////////////////////////////////////////////////////////////////////////////
//! @brief  : return true if find intersectant rectangle with obj in detected_objects_buffer_norm 
////////////////////////////////////////////////////////////////////////////////
inline bool has_intersect(
	 const int4 obj // xy: obj position,z: object sum of probe,w win_size
	,__global detected_objects_buffer_norm* detected_obj
	,const int2 img_size
	,int4 *intersect_obj
		){	
	const float2 normalization_factor=get_normalization_factor(img_size);
	float4 norm_obj=obj_normalization(obj,normalization_factor);
	float4 intersect_obj_norm;
	bool b=has_intersect_norm(norm_obj,detected_obj,&intersect_obj_norm);
	*intersect_obj=obj_unnormalization(intersect_obj_norm,img_size);
	return b;
}


//////////////////////////////////////////////////////////////////////////
inline int4 get_detect_rectangle(int2 pos,int win_size){
	return (int4)(pos,pos+win_size);
}
inline float4 get_detect_rectangle_norm(int2 pos,int win_size,const float2 normalization_factor){
	return rectangle_normalization(get_detect_rectangle(pos,win_size),normalization_factor);
}
inline float4 get_object_rectangle_norm(float4 obj,const float2 normalization_factor){	
	return (float4)(obj.xy,obj.xy+(float2)(obj.w)*normalization_factor/max(normalization_factor.x,normalization_factor.y));
}

typedef enum _rect_intersect_type{
	 NOT_INTERSECT
	,HAS_INTERSECT_OBJECTS
	,INCLUDED_BY_OBJECT
	,INCLUDE_OBJECT
}rect_intersect_type;
///////////////////////////////////////////////////////////////////////////////
//! @brief  : compute intersect status between detect_rect_norm and objectes of detected_obj
//            return rect_intersect_type  
//			  inclued_obj_rect_norm is valid only where intersect_status !=NOT_INTERSECT
////////////////////////////////////////////////////////////////////////////////
inline rect_intersect_type find_intersect_rect_norm(
	 const float4 detect_rect_norm 
	,__constant detected_objects_buffer_norm* detected_obj
	,const float2 normalization_factor
	,float4 *inclued_obj_rect_norm
		){
	float4 rect2,detected_obj_i;
	float4 intersect_rect;
	const float detect_rect_width=detect_rect_norm.z-detect_rect_norm.x;
	float min_left_width=detect_rect_width,intersect_left_widh;

	rect_intersect_type intersect_status=NOT_INTERSECT;
	for(int i=0;i<detected_obj->detected_num;++i){
		detected_obj_i=vload4(i,(__constant float*)detected_obj->storage);
		rect2=get_object_rectangle_norm(detected_obj_i,normalization_factor);
		intersect_rect=intersect_rectangle_norm(detect_rect_norm,rect2);
		if(is_valid_rectangle_norm(intersect_rect)){	// is intersectant rectangle between detect_rect_norm and rect2 		
			if(is_equal_rect_norm(intersect_rect,detect_rect_norm)){ // detect_rect_norm be included by rect2
			//if (is_included_by_rect2_norm(detect_rect_norm,rect2)){
				//DEBUG_LOG("get_object_rectangle_norm detected_obj_i=(%v4f),rect2=(%v4f)\n",detected_obj_i,rect2);
				//DEBUG_LOG("find_intersect_rect_norm:detect rect (%v4f) INCLUDED by (%v4f)\n",detect_rect_norm,rect2);			
				*inclued_obj_rect_norm=rect2;
				return INCLUDED_BY_OBJECT;

			}else if (is_equal_rect_norm(intersect_rect,rect2)){ // detect_rect_norm include rect2
			//}else if (is_included_by_rect2_norm(rect2,detect_rect_norm)){
				//DEBUG_LOG("get_object_rectangle_norm detected_obj_i=(%v4f),rect2=(%v4f)\n",detected_obj_i,rect2);
				//DEBUG_LOG("find_intersect_rect_norm:detect rect (%v4f) INCLUDED by (%v4f)\n",detect_rect_norm,rect2);			
				*inclued_obj_rect_norm=rect2;
				return INCLUDE_OBJECT;
			}else{
				if( NOT_INTERSECT==intersect_status )
					intersect_status=HAS_INTERSECT_OBJECTS;
				if( (intersect_left_widh=detect_rect_width-(intersect_rect.z-intersect_rect.x)) < min_left_width ){
					// intersect_rect is more width rect than max_width_intersect_rect,replace it
					min_left_width = intersect_left_widh;
					*inclued_obj_rect_norm=rect2;					
					if(.0f==min_left_width||0==(int)(min_left_width/normalization_factor.x))
						break; 
				}
			}
		}
	}
	return intersect_status;
}
///////////////////////////////////////////////////////////////////////////////
//! @brief  : create hit mask for rectange  exclusive intersect rect between detect rectange and detected object
////////////////////////////////////////////////////////////////////////////////
inline VECTOR_UCHAR create_hit_mask(int left_win_num,bool is_left_mask){
	uchar16 utemp=(uchar16)(0);
	utemp.s1=0xff;
	uchar16 uc16_mask;	
	switch(left_win_num){
	case 0:
		uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1));
		break;
	case 1:					
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 2:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 3:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 4:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 5:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 6:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 7:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0));
		}
		break;
	case 8:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0));
		}
		break;
	case 9:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0));
		}
		break;
	case 10:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0));
		}
		break;
	case 11:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,0,1,1,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0));
		}
		break;
	case 12:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0));
		}
		break;
	case 13:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0));
		}
		break;
	case 14:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0));
		}
		break;
	case 15:
		if(is_left_mask){
			uc16_mask=shuffle(utemp,(uchar16)(0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1));
		}else{
			uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0));
		}
		break;
	default:
		uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1));
		break;
	}
#if V_TYPE==16
	return uc16_mask;
#elif V_TYPE==8
	return is_left_mask?uc16_mask.s89ABCDEF:uc16_mask.s01234567;
#elif V_TYPE==4
	return is_left_mask?uc16_mask.sCDEF:uc16_mask.s0123;
#elif V_TYPE==2
	return is_left_mask?uc16_mask.s01:uc16_mask.sEF;
#endif
}

///////////////////////////////////////////////////////////////////////////////
//! @brief  : create hit mask for rectange  exclusive intersect rect between detect rectange and detected object
////////////////////////////////////////////////////////////////////////////////
inline VECTOR_UCHAR create_step_mask(int step){
	uchar16 utemp=(uchar16)(0);
	utemp.s1=0xff;
	uchar16 uc16_mask;	
	switch(step&0x1f){
	case 1:
		uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1));
		break;
	case 2:
		uc16_mask=shuffle(utemp,(uchar16)(1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0));
		break;
	case 4:
		uc16_mask=shuffle(utemp,(uchar16)(1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0));
		break;
	case 8:
		uc16_mask=shuffle(utemp,(uchar16)(1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0));
		break;
	case 16:
		uc16_mask=shuffle(utemp,(uchar16)(1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0));
		break;
	default:
		uc16_mask=shuffle(utemp,(uchar16)(1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1));
		break;
	}
#if V_TYPE==16
	return uc16_mask;
#elif V_TYPE==8
	return uc16_mask.s01234567;
#elif V_TYPE==4
	return uc16_mask.s0123;
#elif V_TYPE==2
	return uc16_mask.s01;
#endif
}
///////////////////////////////////////////////////////////////////////////////
//! @brief  : return true if detect rectangle included by one of detected objects rectangle in detected_objects_buffer_norm 
////////////////////////////////////////////////////////////////////////////////
inline rect_intersect_type find_intersect_rect(
	 const int4 detect_rect 
	,__constant detected_objects_buffer_norm* detected_obj
	,const int2 img_size
	,const float2 normalization_factor
	,int4 *inclued_obj_rect
		){
	float4 detect_rect_norm=rectangle_normalization(detect_rect,normalization_factor);
	float4 inclued_obj_rect_norm;
	rect_intersect_type intersect_status=find_intersect_rect_norm(detect_rect_norm,detected_obj,normalization_factor,&inclued_obj_rect_norm);
	*inclued_obj_rect=rectangle_unnormalization(inclued_obj_rect_norm,img_size);
	return intersect_status;	
}

inline int4 win_startx(int y,int win_size,int row_stride){
	int4 index;
	index.s0=y*row_stride;						// A1
	index.s1=index.s0 + win_size;				// A2
	index.s2=(y+win_size)*row_stride;			// A3
	index.s3=index.s2 + win_size;				// A4
	return index;
}

inline int8 haar_startx(int a1,int row_stride,const feather_cl *feather){
	int8 index;
	index.s0=a1			+ feather->y*row_stride+feather->x; // x1 
	index.s1=index.s0	+ feather->Width;                   // x2
	index.s2=index.s0 	+ (feather->Height>>1)*row_stride;  // x3
	index.s3=index.s2 	+ feather->Width;                   // x4
	index.s4=index.s0	+ feather->Height*row_stride;	    // x5
	index.s5=index.s4 	+ feather->Width;                   // x6
	return index;
}

///////////////////////////////////////////////////////////////////////////////
//! @brief  : compute difference between 'feather' and integral matrix specified by 'detect_win' on 'integral_mat'  	
////////////////////////////////////////////////////////////////////////////////
inline VECTOR_FLOAT get_difference(
		 const __global feather_cl * const feather
		,const matrix_info_cl *im_info
 		,const __global  float * const integral_mat
 		,const uint2 detect_win
 		){
 	uint tt1,tt2,tt3,tt4,tt5,tt6;
	uint x1,x2,x3,x4,x5,x6,x7,x8,x9;
 	switch(feather->RectangleNum){
	case 2:
		//  +-----------------------+
		//  |                       |
		//  |   x1+-----------+x2   |
		//  |     |           |     |
		//  |     |           |     |
		//  |   x3+-----------+x4   |
		//  |     |           |     |
		//  |     |           |     |
		//  |   x5+-----------+x6   |
		//  |                       |
		//  +-----------------------+
		if(feather->Up_Down){
			tt1 = ((detect_win.y + feather->y)*im_info->row_stride);
			tt2 = ((detect_win.y + feather->CorrectNum)*im_info->row_stride);
			tt3 = ((detect_win.y + feather->NonfaceCorrectNum)*im_info->row_stride);
			tt4 = (detect_win.x + feather->Valve);
			tt5 = (detect_win.x + feather->x);
			x1 = tt1 + tt5;
			x2 = tt1 + tt4;
			x3 = tt5 + tt2;
			x5 = tt5 + tt3;
			x4 = tt4 + tt2;
			x6 = tt4 + tt3;
			return VCONVERT(VECTOR_FLOAT,)((VLOAD(0,integral_mat+x6)-VLOAD(0,integral_mat+x5))-(VLOAD(0,integral_mat+x4)-VLOAD(0,integral_mat+x3))*2  +  (VLOAD(0,integral_mat+x2)-VLOAD(0,integral_mat+x1)));
			//return  (x6-x5)-(x4-x3)*2  +  (x2-x1);
		}
		//  +-----------------------+
		//  |    x1     x2    x3    |
		//  |     +-----+-----+     |
		//  |     |     |     |     |
		//  |     |     |     |     |
		//  |     |     |     |     |
		//  |     |     |     |     |
		//  |     |     |     |     |
		//  |     +-----+-----+     |
		//  |    x4     x5    x6    |
		//  +-----------------------+	
		else{
			tt1 = ((detect_win.y + feather->y)*im_info->row_stride);
			tt2 = ((detect_win.y + feather->Valve)*im_info->row_stride);
			tt3 = (detect_win.x + feather->x);
			tt4 = (detect_win.x + feather->CorrectNum);
			tt5 = (detect_win.x + feather->NonfaceCorrectNum);
			x1 = tt1 + tt3;
			x2 = tt1 + tt4;
			x3 = tt1 + tt5;
			x4 = tt2 + tt3;
			x5 = tt2 + tt4;
			x6 = tt2 + tt5;
			return VCONVERT(VECTOR_FLOAT,)((VLOAD(0,integral_mat+x6)-VLOAD(0,integral_mat+x3)) - ((VLOAD(0,integral_mat+x5)-VLOAD(0,integral_mat+x2))*2) + (VLOAD(0,integral_mat+x4)-VLOAD(0,integral_mat+x1)));
			//return (x6-x3) - ((x5-x2)*2) + (x4-x1);
		}	
	case 3:
		//  +-----------------------------+
		//  |    x1     x2    x3    x4    |
		//  |     +-----+-----+-----+     |
		//  |     |     |     |     |     |
		//  |     |     |     |     |     |
		//  |     |     |     |     |     |
		//  |     |     |     |     |     |
		//  |     |     |     |     |     |
		//  |     +-----+-----+---- +     |
		//  |    x5     x6    x7    x8    |
		//  +-----------------------------+
		tt1 = ((detect_win.y + feather->y)*im_info->row_stride);
		tt2 = ((detect_win.y + feather->y+feather->Height)*im_info->row_stride);
		tt3 = (detect_win.x + feather->x);
		tt4 = (detect_win.x + feather->CorrectNum);
		tt5 = (detect_win.x + feather->NonfaceCorrectNum);
		tt6 = (detect_win.x + feather->Valve);
		x1 = tt1 + tt3;
		x2 = tt1 + tt4;
		x3 = tt1 + tt5;
		x4 = tt1 + tt6;
		x5 = tt2 + tt3;
		x6 = tt2 + tt4;
		x7 = tt2 + tt5;
		x8 = tt2 + tt6;
		return VCONVERT(VECTOR_FLOAT,)((VLOAD(0,integral_mat+x6)-VLOAD(0,integral_mat+x2)+VLOAD(0,integral_mat+x3)-VLOAD(0,integral_mat+x7))*3+VLOAD(0,integral_mat+x1)-VLOAD(0,integral_mat+x5)+VLOAD(0,integral_mat+x8)-VLOAD(0,integral_mat+x4));
		//return (x6-x2+x3-x7)*3+x1-x5+x8-x4;
	case 4:
		//  +-----------------------+
		//  |    x1     x2    x3    |
		//  |     +-----+-----+     |
		//  |     |     |     |     |
		//  |     |     |     |     |
		//  |   x4+---x5+---x6+     |
		//  |     |     |     |     |
		//  |     |     |     |     |
		//  |     +-----+-----+     |
		//  |    x7     x8    x9    |
		//  +-----------------------+	
		tt1 = ((detect_win.y + feather->y)*im_info->row_stride);
		tt2 = ((detect_win.y + feather->y+(feather->unitHeight))*im_info->row_stride);
		tt3 = ((detect_win.y + feather->y+feather->Height)*im_info->row_stride);
		tt4 = (detect_win.x + feather->x);
		tt5 = (detect_win.x + feather->x+(feather->unitWidth));
		tt6 = (detect_win.x + feather->x+feather->Width);
		x1 = tt1 + tt4;
		x2 = tt1 + tt5;
		x3 = tt1 + tt6;
		x4 = tt2 + tt4;
		x5 = tt2 + tt5;
		x6 = tt2 + tt6;
		x7 = tt3 + tt4;
		x8 = tt3 + tt5;
		x9 = tt3 + tt6;
		return VCONVERT(VECTOR_FLOAT,)( 
			((((VLOAD(0,integral_mat+x5)*2-VLOAD(0,integral_mat+x4)-VLOAD(0,integral_mat+x8)-VLOAD(0,integral_mat+x6)-VLOAD(0,integral_mat+x2))*2) + VLOAD(0,integral_mat+x1) + VLOAD(0,integral_mat+x9))
			+
			(VLOAD(0,integral_mat+x7) +VLOAD(0,integral_mat+x3)))
			);
		//return 
		//	((((x5*2-x4-x8-x6-x2)*2) + x1 + x9)
		//	+
		//	(x7 +x3));
	default:
		return (VECTOR_FLOAT)(0);
	} 	
 }
///////////////////////////////////////////////////////////////////////////////
//! @brief  : compute difference between 'feather' and 'variance' ,'difference'	
////////////////////////////////////////////////////////////////////////////////
inline void compute_correct(
		 VECTOR_WEIGHT_TYPE *const correct
		,const __global feather_cl * const feather
 		,const float weight   // weight of 'feather'
		,const VECTOR_FLOAT variance  
		,const VECTOR_FLOAT difference
 		){ 		
	if(feather->Sign){
		if (feather->DValve > 0)	{
			correct[0]+= VCONVERT(SELECT_WEIGHT_TYPE,)( (difference > 0.0f)&&((pown(feather->DValve,2) * variance) < pown(difference, 2) ) )? weight:(float)(0.0);
		}else{
			correct[0]+= VCONVERT(SELECT_WEIGHT_TYPE,)( ((difference <= 0)&&(pown(feather->DValve,2) * variance > pown(difference, 2) ))||(difference > 0))? weight:(float)(0.0);
		}
	}else	{
		if (feather->DValve > 0){
			correct[0]+= VCONVERT(SELECT_WEIGHT_TYPE,)( ((difference > 0)&&(pown(feather->DValve,2) * variance > pown(difference, 2)))||(difference <= 0))? weight:(float)(0.0);
		}else	{
			correct[0]+= VCONVERT(SELECT_WEIGHT_TYPE,)( (difference <= 0)&&(pown(feather->DValve,2) * variance < pown(difference, 2)))? weight:(float)(0.0);
		}
	} 		
 }

///////////////////////////////////////////////////////////////////////////////
//! @brief  : haar filter	
// 					all of matrix must align with V_TYPE
////////////////////////////////////////////////////////////////////////////////
// +-------------------------------------------+
// |                                           |
// |                                           |
// |      A1+-----------------------+A2        |
// |        |                       |          |
// |        |   x1+-----------+x2   |          |
// |        |     |           |     |          |
// |        |     |           |     |          |
// |        |   x3+-----------+x4   |          |
// |        |     |           |     |          |
// |        |     |           |     |          |
// |        |   x5+-----------+x6   |          |
// |        |                       |          |
// |      A3+-----------------------+A4        |
// |                                           |
// |                                           |
// |                                           |
// +-------------------------------------------+
__kernel void haar_filter( 
 		 __constant detected_objects_buffer_norm* detected_obj_buf_ptr 
  		,__constant casinfo_base_float* const cas
 		,const __global feather_cl* feathers
 		,matrix_info_cl im_info
 		,const __global  float *integ_mat
 		,const __global  float *integ_sqare_mat
 		,matrix_info_cl om_info
 		,__global uchar* hit_mat
 		,const haar_filter_const_param const_param		
 		 ){
	const __global feather_cl * const start_fea_ptr=feathers+const_param.detect_scale_level*cas->FeatherNumPerCas; // get feathers start pointer by 'detect_scale_level'
	
	const int global_id=(int)get_global_id(0);

	const int step_y	= (int)gf_get_divisor(om_info.height,(int)get_global_size(0));
	const int start_y	= (int)get_global_id(0)*step_y;
	const char V_SHIFT_NUM=7-clz((uchar)V_TYPE);
	const int row_stride_v	= CEIL_DIV(om_info.row_stride,V_TYPE);	
	const int end_y = min((int)om_info.height,start_y+step_y);
	const int enc_x_v=CEIL_DIV(om_info.width,V_TYPE);	// align up V_TYPE
	const int2 img_size=(int2)(im_info.width,im_info.height);
	//sample_step is 1/2/4/8/16
	const VECTOR_CHAR step_mask=VAS(VECTOR_CHAR)(create_step_mask(const_param.sample_step));
	__global VECTOR_UCHAR* hit_mat_v =(__global VECTOR_UCHAR*)(hit_mat + start_y*om_info.row_stride);

	int4 integ_index=win_startx(start_y,const_param.win_size,im_info.row_stride);
	
	VECTOR_FLOAT difference,mean,variance;
	VECTOR_CHAR hit,hit_mask;
	VECTOR_WEIGHT_TYPE correct;
	int4 detect_rect,intersect_obj_rect;
	int4 intersect_rect;
	int x_v=0;
	//int block_index;
	//int local_block_size;
	for(int2 detect_win=(int2)(0,(int)start_y);detect_win.y<end_y;
			++detect_win.y
			,hit_mat_v		+= row_stride_v
			,integ_index	+= (int)(im_info.row_stride)
			){

		if(detect_win.y&(int)(const_param.sample_step-1)){
			for(int i=0;i<enc_x_v;++i)hit_mat_v[i]=0;
			continue;
		}
		for( detect_win.x=0,x_v=0;x_v<enc_x_v;++x_v,detect_win.x+=V_TYPE){			
			detect_rect=(int4)(detect_win,detect_win+(int2)(const_param.win_size+V_TYPE,const_param.win_size));
			
			switch(find_intersect_rect(detect_rect,detected_obj_buf_ptr,img_size,const_param.normalization_factor,&intersect_obj_rect)){		
			case INCLUDED_BY_OBJECT:
			case INCLUDE_OBJECT:
				// NOTE: 
				// ADABOOST's minimum detect windows size (DETECT_WIN_SIZE) is 24, otherwise maximum size of vector data types is 16 in opencl kernel
				// so whether the detect rectangle be included by object or include object, the hit byte will be set 0		
				//DEBUG_LOG("SKIP:detect rect (%v4d) INCLUDED by (%v4d)\n",detect_rect,inclued_obj_rect);
				hit_mat_v[x_v]=(VECTOR_UCHAR)(0);
				continue;	
			case HAS_INTERSECT_OBJECTS:
				hit_mask=(VECTOR_CHAR)(0xff);
				intersect_rect=intersect_rectangle(detect_rect,intersect_obj_rect);
				if(is_valid_rectangle(intersect_rect)){
					int left_win_num=V_TYPE-(intersect_rect.z-intersect_rect.x);
					if(left_win_num<=0){
						hit_mat_v[x_v]=(VECTOR_UCHAR)(0);
						continue;				
					}else{	
						hit_mask=VAS(VECTOR_CHAR)(create_hit_mask(left_win_num,detect_rect.x==intersect_rect.x));
					}
				}
				break;
			default:
				hit_mask=(VECTOR_CHAR)(0xff);
				break;
			}			
			hit_mask&=step_mask;
			mean		= (VLOAD(x_v,integ_mat+integ_index.s3) + VLOAD(x_v,integ_mat+integ_index.s0)  - VLOAD(x_v,integ_mat+integ_index.s1 ) - VLOAD(x_v,integ_mat+integ_index.s2) )*const_param.inv_pixels_num;
			variance	= (VLOAD(x_v,integ_sqare_mat+integ_index.s3)+ VLOAD(x_v,integ_sqare_mat+integ_index.s0) - VLOAD(x_v,integ_sqare_mat+integ_index.s1) - VLOAD(x_v,integ_sqare_mat+integ_index.s2))*const_param.inv_pixels_num-(mean*mean);		
			hit=VCONVERT(VECTOR_CHAR,)(
					   (variance>(float)(VARIANCE_MINVAR*VARIANCE_MINVAR))
					&& (variance<(float)(VARIANCE_MAXVAR*VARIANCE_MAXVAR))					
						)&hit_mask;	
			if(any(hit)){
				// start from first layer
				if(om_info.width-detect_win.x<V_TYPE)
					hit&=VAS(VECTOR_CHAR)(create_hit_mask(om_info.width-detect_win.x,false));
				for(int layer=0,feather_index=0,end_layer=cas->CasNum;layer<end_layer  ;feather_index += cas->FeaNumPerLayer[layer++]){		
					correct = (VECTOR_WEIGHT_TYPE)(0);
					for(int sub_fea = feather_index,end_sub=feather_index+cas->FeaNumPerLayer[layer]; sub_fea < end_sub; ++sub_fea){
						difference=get_difference( 
							 start_fea_ptr+sub_fea
							,&im_info
 							,integ_mat
 							,as_uint2(detect_win)
 							);
						compute_correct( &correct
							,start_fea_ptr+sub_fea
							,cas->Weight[sub_fea]
							,variance
							,difference
							);
					}
					hit&=VCONVERT(VECTOR_CHAR,)(cas->LayerWeight[layer] <= correct);				
					if(all(!hit)){
						break;
					}
				}
			}
			hit_mat_v[x_v]=VAS(VECTOR_UCHAR)(hit);
		}	
	}
}


#define DENSITY_LOCAL_BLOCK_SIZE (CL_DEVICE_LOCAL_MEM_SIZE/sizeof(VECTOR_USHORT)/DENSITY_LOCAL_SIZE)
///////////////////////////////////////////////////////////////////////////////
//! @brief  : compute face density on integral count matrix created by hit matrix	
// 					all of matrix must align with V_TYPE
////////////////////////////////////////////////////////////////////////////////
// +-------------------------------------------+
// |                                           |
// |                                           |
// |      A1+-----------------------+A2        |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |        |                       |          |
// |      A3+-----------------------+A4        |
// |                                           |
// |                                           |
// |                                           |
// +-------------------------------------------+
 __kernel void object_density_filter( 
 		  matrix_info_cl im_info
 		, const __global  ushort *integ_count_mat
 		, matrix_info_cl om_info
 		, __global ushort* density_mat
		,int  face_dist_size
		,ushort sum_threshold	
 		 ){

	const int local_id	= (int)get_local_id(0);	
	__local VECTOR_USHORT local_block[DENSITY_LOCAL_SIZE*DENSITY_LOCAL_BLOCK_SIZE];
	__local VECTOR_USHORT* const local_ptr=(__local VECTOR_USHORT *)(local_block) + local_id*DENSITY_LOCAL_BLOCK_SIZE;
	const int v_step	= gf_get_divisor(om_info.height,get_global_size(0));
	VECTOR_USHORT sum;
	int block_size,block_index;
	const char V_SHIFT_NUM=7-clz((uchar)V_TYPE);
	const int row_stride_v = om_info.row_stride>>V_SHIFT_NUM;
	
	int2 pos =(int2)(0, get_global_id(0)*v_step);	
	int4 index=win_startx(pos.y,face_dist_size,im_info.row_stride);
	__global VECTOR_USHORT* density_mat_v = (__global VECTOR_USHORT*)(density_mat)	 + pos.y*row_stride_v;
	int4 index_x;
	for(int end_y=min((int)om_info.height,pos.y+v_step),end_x_v=CEIL_DIV(om_info.width,V_TYPE)
			;pos.y < end_y
			;++pos.y,density_mat_v+=row_stride_v,index+=(int)im_info.row_stride
			){
#if 1					
		for( int x_v=0;x_v<end_x_v;x_v+=block_size){			
			block_size=min(end_x_v-x_v,(int)DENSITY_LOCAL_BLOCK_SIZE);
			for( index_x=index+(x_v<<V_SHIFT_NUM),block_index = 0;block_index < block_size; ++block_index ){			
				// compute faces sum in each window which size speciialed face_dist_size : A4+A1-A2-A3
				sum=( VLOAD(block_index,integ_count_mat +index_x.s3) + VLOAD(block_index,integ_count_mat+index_x.s0)  
					- VLOAD(block_index,integ_count_mat +index_x.s1) - VLOAD(block_index,integ_count_mat+index_x.s2) );
				// fake object filter by density_const_param.sum_threshold
				local_ptr[block_index]=sum>=sum_threshold?sum:(VECTOR_USHORT)(0);
			}
			//barrier(CLK_LOCAL_MEM_FENCE);
			for( pos.x=x_v,block_index = 0;block_index < block_size; ++block_index,++pos.x ){			
				density_mat_v[pos.x]=local_ptr[block_index];	
			}
		}
#else
		for( int x_v=0;x_v<end_x_v;++x_v){			
			// compute faces sum in each window which size speciialed face_dist_size : A4+A1-A2-A3
			sum=( VLOAD(x_v,integ_count_mat +index.s3) + VLOAD(x_v,integ_count_mat+index.s0)  
				- VLOAD(x_v,integ_count_mat +index.s1) - VLOAD(x_v,integ_count_mat+index.s2) );
			// fake object filter by density_const_param.sum_threshold
			density_mat_v[x_v]=sum>=sum_threshold?sum:(VECTOR_USHORT)(0);
		}
#endif
	}
}

#define MAX_OBJECT_NUM ((CL_DEVICE_LOCAL_MEM_SIZE)/sizeof(int4))

inline bool find_intersect(
	 const int4 probe // xy: prob position,z: object sum of probe
	,const int win_size
	,__local int4* detected_obj
	,const int object_num
	,int2 *diff
		){
	for(int i=0;i<object_num;++i){
		*diff=probe.xy-detected_obj[i].xy;
		if (abs((*diff).x)<win_size && abs((*diff).y)<win_size){
			if(probe.z>detected_obj[i].z){
				// replace dectectd object info  with probe which having  max object sum				
				detected_obj[i]=probe;
			}
			return true;
		}		
	}
	return false;
}

inline int4 find_max_sum_in_dist_win(
		  const matrix_info_cl *im_info
		, const int2 zero_pos
 		, __global  ushort *const dist_win_ptr
		, const int2 dist_win_search_size
		){
	__global  ushort *line_ptr=dist_win_ptr;
	int4 max=(int4)((int2)(0,0),(int2)(*line_ptr)); // initlize max with value on (0,0)
	for(int2 pos=(int2)(0,0); pos.y<dist_win_search_size.y; ++pos.y,line_ptr+=im_info->row_stride){
		for(;pos.x<dist_win_search_size.x;++pos.x){
			if(line_ptr[pos.x]){
				if(line_ptr[pos.x]>max.z){
					DEBUG_LOG("replace (%v2d),sum=%d with R(%v2d),sum=%d\n",zero_pos+max.xy,max.z,pos,line_ptr[pos.x]);
					// clear last max
					dist_win_ptr[max.x+max.y*im_info->row_stride]=0;
					// save current max 
					max=(int4)(pos,(int2)(line_ptr[pos.x],0));
				}else
					line_ptr[pos.x]=0;
			}
		}
	}
	DEBUG_LOG("found max (%v2d),R(%v2d),sum=%d,dist_win_search_size=(%v2d)\n",zero_pos+max.xy,max.xy,max.z,dist_win_search_size);
	max.xy+=zero_pos;
	return max;
}


inline bool copy_detected_obj_to_host(const __local int4* detected_obj
	, __global detected_objects_buffer_norm* out
	, int object_num	
	, const int win_size
	, const int dist_size
	, const int2 img_size){	
	if(out->detected_num+object_num<MAX_DETECTED_OBJECT_NUM){
		float4 norm_obj;
		const float2 normalization_factor=1/convert_float2(img_size);
		int4 intersect_obj;
		int new_count=0;
		for(int i=0; i < object_num  ; ++i){
			detected_obj[i].xy+=(dist_size>>1);	
			detected_obj[i].w=win_size;			
			if(has_intersect(detected_obj[i],out,img_size,&intersect_obj)){
				DEBUG_LOG("no:%d:INTERSECT RECT (%v2d),sum=%d,win_size=%d VS (%v2d),sum=%d,win_size=%d\n"
					,i
					,detected_obj[i].xy,detected_obj[i].z,detected_obj[i].w
					,intersect_obj.xy  ,intersect_obj.z ,intersect_obj.w);			
			}else{
				DEBUG_LOG("no:%d: pos(%v2d),sum=%d,win_size=%d\n",i,detected_obj[i].xy,detected_obj[i].z,detected_obj[i].w);
				norm_obj=obj_normalization(detected_obj[i],normalization_factor);
				DEBUG_LOG("no:%d, pos(%v2f),sum=%f,win_size=%f\n",i,norm_obj.xy,norm_obj.z,norm_obj.w);
				out->storage[out->detected_num+(new_count++)]=norm_obj; // throw exception on cl_mem_flags is CL_MEM_USE_HOST_PTR
			}
		}
		DEBUG_LOG("new found %d objectes, %d duplicated objects \n",new_count,object_num-new_count);
		out->detected_num += new_count;	
		out->status=KERNEL_OK;
		return true;
	}else{
		out->status=KERNEL_OUT_INDEX;
		return false;
	}
}

///////////////////////////////////////////////////////////////////////////////
//! @brief  : object cluster by object density matrix
///////////////////////////////////////////////////////////////////////////////
__kernel void object_cluster(
 		  matrix_info_cl density_mat_info
 		, __global ushort* density_mat
		, __global detected_objects_buffer_norm* out
 		, const cluster_filter_const_param const_param		
 		 ){
	int object_num=0;// detected object number	
	const char V_SHIFT_NUM=7-clz((uchar)V_TYPE);
	const int row_stride_v = density_mat_info.row_stride>>V_SHIFT_NUM;
	__local int4 detected_obj[MAX_OBJECT_NUM];// save detected object info: x,y object location,z, object sum	

	__global  ushort *line_ptr=density_mat;	
	__global  VECTOR_USHORT *line_v_ptr=(__global  VECTOR_USHORT *)density_mat;
	const int2 mat_size=(int2)((int)density_mat_info.width,(int)density_mat_info.height);
	for(int2 pos=(int2)(0,0);pos.y<density_mat_info.height;++pos.y,line_ptr+=density_mat_info.row_stride,line_v_ptr+=row_stride_v){		
		for(pos.x=0;pos.x<density_mat_info.width;){
			if(!(pos.x&(V_TYPE-1))){
				if(all((VECTOR_USHORT)(0)==line_v_ptr[pos.x>>V_SHIFT_NUM])){
					pos.x+=V_TYPE;
					continue;
				}
			}			
			if(!line_ptr[pos.x]){
				++pos.x;
				continue;
			}else{ 
				int2 diff;
				if(find_intersect((int4)(pos,(int2)((int)line_ptr[pos.x])),const_param.win_size,detected_obj,object_num,&diff)){
					pos.x+=const_param.win_size-diff.x;
					continue;
				}
			}
			int4 max_sum=find_max_sum_in_dist_win(
				&density_mat_info
				, pos
				, line_ptr+pos.x
				, min(mat_size-pos,const_param.face_dist_size));	
					
			if(max_sum.z){
				detected_obj[object_num++]=max_sum;				
				pos.x+=const_param.win_size;
				if(object_num>=MAX_OBJECT_NUM){
					// return error
					out->status=KERNEL_OUT_LOCAL_INDEX;
					return;
				}
			}else{
				// return error
				out->status=KERNEL_ZERO_OBJ_SUM;
				return;
			}							
		}
	}
	DEBUG_LOG("found objects %d\n",object_num);
	if(object_num>0)
		copy_detected_obj_to_host(detected_obj, out, object_num,const_param.win_size,const_param.face_dist_size, const_param.img_size);
}
